/*
 * To change this template, choose Tools | Templates
 * and open the template in the editor.
 */

package edu.wpi.first.wpilibj.templates;

import com.sun.squawk.util.StringTokenizer;

import edu.wpi.first.wpilibj.Timer;

import file.AutonomousScript;
import file.Configurable;
import file.Configuration;
import file.DataRecorder;

/**
 *
 * @author Mentors  Carl Burger
 */

/**
 * The AutonomousController class controls the robot using the specified
 * autonomous script
 *
 * Command      Parameter(s)
 * FollowLine   type={straight | left | right} speed=  gain=  proximity=
 * Drive        speed=  heading=  distance=  time=
 * MoveElevator position={top | middle | bottom | feeder}  offset={true | false}
 * PlaceOnPeg
 * Delay        secs=
 * Halt
 */
public class AutonomousController implements Configurable{

    // Autonomous Mode script and variables

    AutonomousScript autoScript;
    Drivetrain       drivetrain;
    DriveController  driveController;
    Manipulator      manipulator;
    Elevator         elevator;
    LineFollower     lineFollower;
    Wrist            wrist;

    DataRecorder     dataRecorder;
    MessageCenter    messageCenter;

    // Timers

    Timer   autoTimer;  // used for "delay" autonomous script command


    String  autoCommandLine;
    String  autoCommand = " ";

    boolean followingLine   = false;
    boolean autoCommandBusy = false;
    boolean autoDebug       = false;

    int     autoHeading;
    double  autoSpeed;
    int     autoDistance;
    double  autoTime;
    double  autoGain;
    int     autoProximity;
    double  autoDelay;  // interval to "delay" from autonomous script command

    int previousMode = -1; //used for displaying to messagecenter

    public AutonomousController () {

        autoScript   = AutonomousScript.getInstance();

        drivetrain   = Drivetrain.getInstance();
        driveController = DriveController.getInstance();
        elevator     = Elevator.getInstance();
        wrist        = Wrist.getInstance();
        lineFollower  = LineFollower.getInstance();
        autoTimer    = new Timer();
        dataRecorder = DataRecorder.getInstance();
        manipulator  = Manipulator.getInstance();
        messageCenter = MessageCenter.getInstance();
        
        Configuration.getInstance().register("AutonomousController", this);
    }

    public void runScript () {

        // each script should end with a Halt command so hasNextCommand() should
        // always return true

        if (autoScript.hasNextCommand() == true) {

            // check if the previous command is still in-progress
            if (autoCommandBusy == false) {

                autoCommandLine = autoScript.getNextCommand();

                StringTokenizer st = new StringTokenizer(autoCommandLine);
                autoCommand = st.nextToken();
                System.out.println("Autonomous command " + autoCommand);

                if (autoCommand.equalsIgnoreCase("FollowLine")) {
//                    autoCommandBusy = processFollowLineCommand(st);
                    followingLine = processFollowLineCommand(st);
                    // allow other commands to be performed while following the line
                    autoCommandBusy = true;
                }

                else if(autoCommand.equalsIgnoreCase("Drive")) {
                    autoCommandBusy = processDriveCommand(st);
                }
                
                else if(autoCommand.equalsIgnoreCase("Align")){
                    autoCommandBusy = processAlignCommand();
                }

                else if(autoCommand.equalsIgnoreCase("PositionWrist")){
                    autoCommandBusy = processPositionWristCommand();
                }

                else if(autoCommand.equalsIgnoreCase("LowerWrist")){
                    autoCommandBusy = processLowerWristCommand();
                }

                else if (autoCommand.equalsIgnoreCase("MoveElevator")) {
                    autoCommandBusy = processMoveElevatorCommand(st);
                }

                else if (autoCommand.equalsIgnoreCase("PlaceOnPeg")) {
                    autoCommandBusy = processPlaceOnPegCommand();
                }

                else if (autoCommand.equalsIgnoreCase("Delay")) {
                    autoCommandBusy = processDelayCommand(st);
                }

                else if (autoCommand.equalsIgnoreCase("Halt")) {
                    // nothing to do.  Should be the last commnad in a script so
                    // autoScript.hasNextCommand() should return false
                }

                else {
                    System.out.println("Invalid script command - " + autoCommand);
                    autoCommandBusy = false;
                }
            }
            else { // commandBusy is true

                // elevator code checks if robot reached the T
                if (autoCommand.equalsIgnoreCase("FollowLine"))
                {
                    if (lineFollower.onTarget() == true) {
                        autoCommandBusy = false;

                    }
                }

                // check if robot is driving and if the distance was traveled - NOT USED
                // check if robot is driving and if the time was reached
                if(autoCommand.equalsIgnoreCase("Drive"))
                {
                    if (autoTimer.get() >= autoTime) {
                        System.out.println("Drive finished: " + autoTimer.get());
                        autoTimer.stop();
                        drivetrain.drive(0, 0, 0);
                        autoCommandBusy = false;
                    }
//                    if (drivetrain.getFrontLeftDistance() >= autoDistance){
//                    // stop driving
//                    drivetrain.drive(0, 0, 0);
//
//                    autoCommandBusy = false;
//                    }
                }
                
                else if(autoCommand.equalsIgnoreCase("Align")){
                    driveController.checkProgress();
                    autoCommandBusy = !driveController.onTarget();
                }

                else if(autoCommand.equalsIgnoreCase("PositionWrist")){
                    if (wrist.onTarget() == true || autoDebug == true)
                    {
                        wrist.hang();
                        autoCommandBusy = false;
                    }
                }

                else if(autoCommand.equalsIgnoreCase("LowerWrist")){
                    if (wrist.onTarget() == true || autoDebug == true)
                    {
                        autoCommandBusy = false;
                    }
                }

                // check if a delay is in effect and if it expired
                else if (autoCommand.equalsIgnoreCase("Delay"))
                {
                    if (autoTimer.get() >= autoDelay) {
                        System.out.println("Delay finished: " + autoTimer.get());
                        autoTimer.stop();
                        autoCommandBusy = false;
                    }
                }

                else if (autoCommand.equalsIgnoreCase("MoveElevator"))
                {
                    if (elevator.atPosition() == true || autoDebug == true) {
                        autoCommandBusy = false;
//                        wrist.hang();  // when elevator is busy, let script do next command
                    }
                }

                else if (autoCommand.equalsIgnoreCase("PlaceOnPeg"))
                {
                    autoCommandBusy = !(manipulator.isFinished() || autoDebug == true);
                }
            } // end if autoCommandBusy
        } // end if hasNextCommand
    }
    
    
    private boolean processFollowLineCommand(StringTokenizer st) {

        boolean  validCommand = true;   // assume no errors
        String[] param;                 // holds param name and value

        String   path      = "";        // empty string if param not found
        String   speed     = "";        // empty string if param not found
        String   gain      = "";        // empty string if param not found
        String   proximity = "";        // empty string if param not found

        // get type, speed, gain, proximity

        int      numberOfParams = st.countTokens();

        //System.out.println("FollowLine numberOfParams " + numberOfParams);
        
        for (int i = 0; i  < numberOfParams; i++) {

            param = getParam(st.nextToken());

            if (param[0].equalsIgnoreCase("path")) {
                path = param[1];
                
            } else if (param[0].equalsIgnoreCase("speed")) {
                speed = param[1];

            } else if (param[0].equalsIgnoreCase("gain")) {
                gain = param[1];

            } else if (param[0].equalsIgnoreCase("proximity")) {
                proximity = param[1];

            } else {
                System.out.println("Invalid autoscript parameter (FollowLine): " + param[0]);
            }
        }

        try
        {
            autoSpeed     = Double.parseDouble( speed );
            autoGain      = Double.parseDouble( gain );
            autoProximity = Integer.parseInt( proximity );

            if (path.equalsIgnoreCase("straight") || path.equalsIgnoreCase("left") || path.equalsIgnoreCase("right")) {
                lineFollower.setControls(path, autoSpeed, autoGain, autoProximity);
                lineFollower.enable();

            } else {
                System.out.println("Invalid autoscript param value (FollowLine): " + path);
            }
        }
        catch (NumberFormatException e)
        {
            System.out.println("Invalid autoscript parameter value (FollowLine)");
            validCommand = false;
        }
        return validCommand;
    }

    
    private boolean processDriveCommand (StringTokenizer st) {

        boolean  validCommand = true;   // assume no errors
        String[] param;                 // holds param name and value

        String   speed     = "";        // empty string if param not found
        String   heading   = "";        // empty string if param not found
        String   distance  = "";        // empty string if param not found
        String   time      = "";        // empty string if param not found

        // get speed, heading, distance, time

        int      numberOfParams = st.countTokens();

        for (int i = 0; i  < numberOfParams; i++) {

            param = getParam(st.nextToken());

            if (param[0].equalsIgnoreCase("speed")) {
                speed = param[1];

            } else if (param[0].equalsIgnoreCase("heading")) {
                heading = param[1];

            } else if (param[0].equalsIgnoreCase("distance")) {
                distance = param[1];

            } else if (param[0].equalsIgnoreCase("time")) {
                time = param[1];

            } else {
                System.out.println("Invalid autoscript parameter (Drive): " + param[0]);
            }
        }

        try
        {
            autoSpeed     = Double.parseDouble( speed );
            autoHeading   = Integer.parseInt( heading );

            if (distance.equalsIgnoreCase("")) {
                autoDistance = 0;
            } else {
                autoDistance = Integer.parseInt( distance );
            }

            if (time.equalsIgnoreCase("")) {
                autoTime = 0.0;
            } else {
                autoTime = Double.parseDouble( time );
            }

            if (distance.equalsIgnoreCase("") && time.equalsIgnoreCase("")) {
                System.out.println("Invalid autoscript param value (Drive): ");

            } else {

                drivetrain.resetEncoders();

                //  North = 0 degrees
                //  East = 90 degrees
                //  South = 180 degrees
                //  West = -90 degrees

                // start driving
                drivetrain.drive(autoSpeed, autoHeading, 0);

                // drive command must specify a time for 2011
                autoTimer.reset();
                autoTimer.start();

            }
        }
        catch (NumberFormatException e)
        {
            System.out.println("Invalid autoscript parameter value (Drive)");
            validCommand = false;
        }
        return validCommand;
    }
    
    private boolean processAlignCommand()
    {
        driveController.setHeading(0);
        return true;
    }

    private boolean processPositionWristCommand()
    {
        wrist.hang();
        return false;  // allow next script command to be processed
//        wrist.flick();
//        return true;
    }

    private boolean processLowerWristCommand()
    {
        wrist.flick();
        autoTimer.reset();
        return true;
    }
    
    private boolean processMoveElevatorCommand (StringTokenizer st) {

        boolean  commandBusy      = false;   // assume no errors
        String[] param;                     // holds param name and value
        String   elevatorPosition = "";     // empty string if param not found
        boolean  elevatorOffset   = true;   // default to higher peg
        
        // wait until robot is in position

//        if (lineFollower.onTarget())
//        {
            // get position and offset parameters
            int numberOfParams = st.countTokens();

            for (int i = 0; i  < numberOfParams; i++) {

                param = getParam(st.nextToken());

                if (param[0].equalsIgnoreCase("position")) {
                    elevatorPosition = param[1];

                } else if (param[0].equalsIgnoreCase("offset")) {
                    elevatorOffset = param[1].equalsIgnoreCase("true") ? true : false;
                }
                else {
                    System.out.println("Invalid autoscript param (MoveElevator): " + param[0]);
                }
            }

            if (elevatorPosition.equalsIgnoreCase("top")) {
                elevator.moveToTopPeg(elevatorOffset);

            } else if (elevatorPosition.equalsIgnoreCase("middle")) {
                elevator.moveToMiddlePeg(elevatorOffset);

            } else if (elevatorPosition.equalsIgnoreCase("bottom")) {
                elevator.moveToBottomPeg(elevatorOffset);
                commandBusy = true;  // let elevator secure shape on peg

            } else if (elevatorPosition.equalsIgnoreCase("feeder")) {
                elevator.moveToFeeder();

            } else {
                System.out.println("Invalid autoscript param value (MoveElevator): " + elevatorPosition);
                commandBusy = false;
            }
//        }
        return commandBusy;
    }


    private boolean processPlaceOnPegCommand() {

        manipulator.placeOnPeg();
        return false;
    }


    private boolean processDelayCommand(StringTokenizer st) {

        boolean  validCommand = true;   // assume no errors
        String[] param;                 // holds param name and value
        String   seconds      = "";     // empty string if param not found

        // get seconds parameter

        int numberOfParams = st.countTokens();

        for (int i = 0; i  < numberOfParams; i++) {

            param = getParam(st.nextToken());

            if (param[0].equalsIgnoreCase("seconds") || param[0].equalsIgnoreCase("secs")) {
                seconds = param[1];
            }
            else {
                System.out.println("Invalid autoscript param (Delay): " + param[0]);
            }
        }

        try
        {
            autoDelay = Double.parseDouble( seconds );

            autoTimer.reset();
            autoTimer.start();
            System.out.println("Delay " + autoDelay + " seconds.");
        }
        catch (NumberFormatException e)
        {
            System.out.println("Invalid autoscript param value (Delay): " + seconds);
            validCommand = false;
        }

        return validCommand;
    }


    private String[] getParam(String namedParam) {

        int      equalSignIndex = namedParam.indexOf("=");
        String[] param;

	//to store the parameter name and value
	param = new String[2];

        if (equalSignIndex != -1) {
            param[0] = namedParam.substring(0, equalSignIndex);
            param[1] = namedParam.substring(equalSignIndex + 1, namedParam.length());
        }  else {
            param[0] = "";
            param[1] = "";
        }

        return param;
    }

    public void outputStatus(){
        int mode = autoScript.getMode();

        if(mode != previousMode){
            messageCenter.println(MessageCenter.kFirstScreen,
                    MessageCenter.kLine1,
                    "Auto Mode: " + mode);

            previousMode = mode;
        }
    }

    public void recordStatus(){
        dataRecorder.record("automode",autoScript.getMode());
        dataRecorder.record("autoCommand", autoCommand);
        
    }

    public void update(String name, String[] values) {
        if(name.equals("Debug")){
            autoDebug = values[0].equalsIgnoreCase("True");
        }
    }
}